#include "../_includes.h"

void motor_enc_disp() {
  const color clr[6] = {color(ClrViolet),    color(ClrLightGoldenrodYellow),
                        color(ClrPeachPuff), color::cyan,
                        color::cyan,         color::green};
  int line_y = 218;
  int n_clr = 0;
  screen_set_pen_font(fontType::mono20);
  for (auto iter : ActionUnit::vec_ptr_all_instacnes) {
    if (iter->get_name() == "chassis_l" || iter->get_name() == "chassis_r")
      continue;
    if (iter->b_screen_enc_disp) {
      screen_set_pen_color(clr[n_clr]);
      Print(0, line_y -= 20, true, "%-6s enc:%-5d spd:%-3d",
            iter->get_name_c_str(), (int)iter->get_enc(),
            (int)iter->get_actual_velocity_pct());
      n_clr++;
    }
  }

  // print dist-mm of chassis
  line_y = 200;
  screen_set_pen_color(color::green);
  static char str[20];
  sprintf(str, "%d mm", (int)chassis.get_dist_mm(-1));
  Print(0, line_y += 18, true, "chassis  LB: %-6s", str);
  sprintf(str, "%d mm", (int)chassis.get_dist_mm(1));
  Print(0, line_y += 18, true, "chassis  RB: %-6s", str);
}

void disp_page_sensors() {
  screen_set_pen_color(color::cyan);
  screen_set_pen_font(fontType::prop20);
  int line_y = 0;
  Print(60, line_y += 20, " Sensor Data ");

  screen_set_pen_color(color::orange);
  screen_set_pen_font(fontType::mono20);
  line_y = 20;
  if (imu.installed()) {
    Print(0, line_y += 20, "gyro: %-5.2f", get_gyro());
    Print(0, line_y += 20, "pitch:%-5.2f", get_pitch());
  } else
    Print(0, line_y += 20, "imu: Uninstalled ");
  //////////////////////////////////////////////////////////////////////////////////////
  motor_enc_disp();
}
